#include "BulletSimulator.h"

#include "BasicDebug.h"
#include "Creature.h"

#include <BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h>
#include <BulletDynamics/ConstraintSolver/btTypedConstraint.h>
#include <BulletDynamics/Dynamics/btRigidBody.h>
#include <iostream>
#include <QtGui/QApplication>
#include <QtCore/QThread>

extern ContactAddedCallback gContactAddedCallback;

namespace jcrada {
    BulletSimulator* BulletSimulator::_singleton = NULL;

    BulletSimulator::BulletSimulator() :
    _broadphase(NULL),
    _collision_configuration(NULL),
    _collision_dispatcher(NULL),
    _constraint_solver(NULL),
    _world(NULL),
    _ground_shape(NULL),
    _ground_motion_state(NULL),
    _ground_rigid_body(NULL),
    _max_proxies(1024),
    _timesteps_per_second(24),
    _current_creature(NULL),
    _lock(new QReadWriteLock(QReadWriteLock::Recursive)),
    _hit_the_ground(false),
    _gravity(-9.80665),
    _seconds_for_stabilization(3) {
        for (int i = 0; i < 3; ++i) {
            _min_xyz[i] = -1000;
            _max_xyz[i] = 1000;
        }
    }

    BulletSimulator::~BulletSimulator() {
        destroyPhysics();
    }

    void BulletSimulator::initPhysics() {
        getLock().lockForWrite();
        btVector3 world_min_aabb(_min_xyz[0], _min_xyz[1], _min_xyz[2]);
        btVector3 world_max_aabb(_max_xyz[0], _max_xyz[1], _max_xyz[2]);

        _broadphase = new bt32BitAxisSweep3(world_min_aabb, world_max_aabb, getMaxProxies());
        _collision_configuration = new btDefaultCollisionConfiguration();
        _collision_dispatcher = new btCollisionDispatcher(_collision_configuration);
        _constraint_solver = new btSequentialImpulseConstraintSolver();
        _constraint_solver->setRandSeed(DEFAULT_SEED_CONSTRAINT_SOLVER);

        _world = new btDiscreteDynamicsWorld(_collision_dispatcher, _broadphase,
                _constraint_solver, _collision_configuration);
        btVector3 g(0.0, getGravity(), 0.0);
        _world->setGravity(g);

        _ground_shape = new btStaticPlaneShape(btVector3(0, 1, 0), 1);
        _ground_motion_state = new btDefaultMotionState(btTransform(btQuaternion(0, 0, 0, 1), btVector3(0, -1, 0)));
        btRigidBody::btRigidBodyConstructionInfo gnd_cinf(0, _ground_motion_state,
                _ground_shape, btVector3(0, 0, 0));

        _ground_rigid_body = new btRigidBody(gnd_cinf);
        _ground_rigid_body->setCollisionFlags(_ground_rigid_body->getCollisionFlags() |
                btRigidBody::CF_CUSTOM_MATERIAL_CALLBACK);
        _world->addRigidBody(_ground_rigid_body);
        _current_creature = NULL;
        gContactAddedCallback = customContactAddedCallback;
        getLock().unlock();
    }

    void BulletSimulator::destroyPhysics() {
        getLock().lockForWrite();
        for (int i = 0; i < getWorld().getCollisionObjectArray().size(); ++i) {
            getWorld().removeCollisionObject(getWorld().getCollisionObjectArray()[i]);
        }
        delete _ground_rigid_body;
        delete _ground_motion_state;
        delete _ground_shape;

        delete _world;
        delete _constraint_solver;
        delete _collision_dispatcher;
        delete _collision_configuration;
        delete _broadphase;
        getLock().unlock();
    }

    void BulletSimulator::reset() {
        getLock().lockForWrite();
        destroyPhysics();
        initPhysics();
        getLock().unlock();
    }

    bool BulletSimulator::customContactAddedCallback(btManifoldPoint& cp,
            const btCollisionObject* colObj0, int partId0, int index0,
            const btCollisionObject* colObj1, int partId1, int index1) {
        //Always return false because there is no friction calculation or any other kind of calculation.
        if (getSingleton().hasHitTheGround()) {
            return false;
        }
        (void) cp;
        (void) partId0;
        (void) index0;
        (void) partId1;
        (void) index1;
        getSingleton().getLock().lockForRead();
        if ((colObj0 && colObj0->isStaticObject()) || (colObj1 && colObj1->isStaticObject())) {
            getSingleton().setHitTheGround(true);
        }
        getSingleton().getLock().unlock();
        return false;
    }

    void BulletSimulator::step() {
        getLock().lockForRead();
        getWorld().stepSimulation(1.0 / getTimestepsPerSecond(), 12);
        getLock().unlock();
    }

    void BulletSimulator::addCreature(Creature& creature) {
        getLock().lockForWrite();
        setHitTheGround(false);
        for (int i = 0; i < creature.getNumberOfBodyParts(); ++i) {
            getWorld().addRigidBody(creature.getBodyPart(i).getRigidBody());
        }
        for (int i = 0; i < creature.getNumberOfConstraints(); ++i) {
            getWorld().addConstraint(creature.getConstraint(i).getConstraint(), false);
        }
        _current_creature = &creature;
        getLock().unlock();
    }

    void BulletSimulator::removeCreature() {
        getLock().lockForWrite();
        for (int i = 0; i < getWorld().getCollisionObjectArray().size(); ++i) {
            if (getWorld().getCollisionObjectArray()[i] != _ground_rigid_body) {
                getWorld().removeCollisionObject(getWorld().getCollisionObjectArray()[i]);
            }
        }
        Creature* creature = currentCreature();
        if (creature) {
            creature->beforeRemove();
            for (int i = 0; i < creature->getNumberOfConstraints(); ++i) {
                getWorld().removeConstraint(creature->getConstraint(i).getConstraint());
            }
            for (int i = 0; i < creature->getNumberOfBodyParts(); ++i) {
                getWorld().removeRigidBody(creature->getBodyPart(i).getRigidBody());
            }
            _current_creature = NULL;
        }
        getLock().unlock();
    }

    void BulletSimulator::setHitTheGround(bool hit) {
        this->_hit_the_ground = hit;
        if (hit) {
            getLock().lockForRead();
            if (currentCreature()) {
                currentCreature()->onHitTheGround();
            }
            getLock().unlock();
        }
    }

    void BulletSimulator::setGravity(double gravity) {
        this->_gravity = gravity;
        if (_world) {
            btVector3 g = getWorld().getGravity();
            g.setY(gravity);
            getWorld().setGravity(g);
        }
    }

    int BulletSimulator::main(int argc, char ** argv) {
        QApplication application(argc, argv);
        return application.exec();
    }
}
